#include "Oracle.h"

Oracle::Oracle(SinglePrm* roadmap_robot1, SinglePrm* roadmap_robot2, CollisionDetector*	collision_detector):
m_roadmap_robot1(roadmap_robot1),
m_roadmap_robot2(roadmap_robot2),
m_collision_detector(collision_detector)
{

}

vector<Point_d> Oracle::get_legal_neighbors(Point_d point)
{
	vector<Point_d> neighbors;

	Point_d robot_1_point(point[0], point[1]);
	int robot_1_point_index = m_roadmap_robot1->get_point_index(robot_1_point);

	Point_d robot_2_point(point[2], point[3]);
	int robot_2_point_index = m_roadmap_robot2->get_point_index(robot_2_point);

	vector<int> robot_1_neighbors = m_roadmap_robot1->get_graph()->get_neighbors(robot_1_point_index);
	vector<int> robot_2_neighbors = m_roadmap_robot2->get_graph()->get_neighbors(robot_2_point_index);

	for (int i = 0; i < robot_1_neighbors.size(); ++i)
	{
		Point_d robot_1_current_point = m_roadmap_robot1->m_all_tree_points[robot_1_neighbors[i]];
		Point_2 robot_1_current_point_2(robot_1_current_point[0], robot_1_current_point[1]);

		for (int j = 0; j < robot_2_neighbors.size(); ++j)
		{
			Point_d robot_2_current_point = m_roadmap_robot2->m_all_tree_points[robot_2_neighbors[j]];
			Point_2 robot_2_current_point_2(robot_2_current_point[0], robot_2_current_point[1]);

			if (m_collision_detector->valid_conf(robot_1_current_point_2, robot_2_current_point_2))
			{
				double temp_array[] = {robot_1_current_point[0],
									   robot_1_current_point[1],
									   robot_2_current_point[0],
									   robot_2_current_point[1]};

				neighbors.push_back(Point_d(4, temp_array, temp_array + 4));
			}
			
		}
	}
	return neighbors;
}
